#include <bsp/clock.h>
#include <rtos/timer.h>

namespace {
// 内核时钟周期计数器
void dwt_init() {
    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
    DWT->CYCCNT = 0;
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
}

// The clock can keep for thousands of years
uint64_t bsp_global_tick;
}  // namespace

namespace bsp {
void clockInit() {
    SystemClockConfig();
    dwt_init();
}

void delay_us(uint32_t delayTime_us) {
    static uint32_t cnt;
    static uint32_t start;
    static uint32_t delay;

    cnt = 0;
    start = DWT->CYCCNT;
    delay = delayTime_us * (SystemCoreClock / 1000000);

    while (DWT->CYCCNT - start < delay) {
        __NOP();
    }
}

void delay_ms(uint32_t delayTime_ms) { delay_us(1000 * delayTime_ms); }

void updateGlobalTick() {
    static bool volatile global_time_lock = false;
    static uint32_t last_cycle_count = 0;
    uint64_t current_cycle_count;
    uint64_t cycles_elapsed;
    if (global_time_lock) {
        return;
    }
    global_time_lock = true;

    current_cycle_count = DWT->CYCCNT;

    if (current_cycle_count >= last_cycle_count) {
        cycles_elapsed = current_cycle_count - last_cycle_count;
    } else {
        cycles_elapsed = (UINT32_MAX - last_cycle_count) + current_cycle_count + 1;
    }
    bsp_global_tick += cycles_elapsed;

    last_cycle_count = current_cycle_count;

    global_time_lock = false;
}

float getGlobalTime_us_fp32() {
    updateGlobalTick();
    return bsp_global_tick * 1000000.0F / SystemCoreClock;
}

uint64_t getGlobalTime_us_i64() {
    updateGlobalTick();
    return bsp_global_tick * 1000000 / SystemCoreClock;
}
}  // namespace bsp

/* ------ Hard Timer | 硬件定时器 ------ */
#include <rtos/Semaphore.h>
#include <functional>
#include <core.h>
#define BSP_HARD_TIMER_TIM TIM8
extern TIM_HandleTypeDef htim8;

namespace os {
namespace {
std::function<void()> hard_timer_cb[6];
Semaphore<SemaphoreType::BINARY> hard_timer_sem[6];
}  // namespace

void HardTimer::Init() { HAL_TIM_Base_Start_IT(&htim8); }

void HardTimer::Start(os::HardTimer::Channel channel, units::time::microsecond_t time,
                      std::function<void()> const& call_back) {
    uint32_t cnt_now;
    uint32_t cnt_tar;

    auto time_out = time.to<uint64_t>();
#if defined(DEBUG)
    if (time_out > 65) {
        // 延迟补偿 65us
        time_out -= 65;
    } else {
        call_back();
        return;
    }
#else
    if (time_out > 55) {
        // 延迟补偿 55us
        time_out -= 55;
    } else {
        call_back();
        return;
    }
#endif

    if (time_out >= UINT16_MAX) {
        osError("HardTimer Over loop");
    }

    cnt_now = BSP_HARD_TIMER_TIM->CNT;
    cnt_tar = cnt_now + time_out; /* 计算捕获的计数器值 */
    switch (channel) {
        case bsp::detail::HardTimerChannel::k1:
            hard_timer_cb[0] = std::move(call_back);

            BSP_HARD_TIMER_TIM->CCR1 = cnt_tar;                            /* 设置捕获比较计数器CC1 */
            BSP_HARD_TIMER_TIM->SR = static_cast<uint16_t>(~TIM_FLAG_CC1); /* 清除CC1中断标志 */
            BSP_HARD_TIMER_TIM->DIER |= TIM_FLAG_CC1;                      /* 使能CC1中断 */
            break;
        case bsp::detail::HardTimerChannel::k2:
            hard_timer_cb[1] = std::move(call_back);

            BSP_HARD_TIMER_TIM->CCR2 = cnt_tar;                            /* 设置捕获比较计数器CC2 */
            BSP_HARD_TIMER_TIM->SR = static_cast<uint16_t>(~TIM_FLAG_CC2); /* 清除CC2中断标志 */
            BSP_HARD_TIMER_TIM->DIER |= TIM_FLAG_CC2;                      /* 使能CC2中断 */
            break;
        case bsp::detail::HardTimerChannel::k3:
            hard_timer_cb[2] = std::move(call_back);

            BSP_HARD_TIMER_TIM->CCR3 = cnt_tar;                            /* 设置捕获比较计数器CC3 */
            BSP_HARD_TIMER_TIM->SR = static_cast<uint16_t>(~TIM_FLAG_CC3); /* 清除CC3中断标志 */
            BSP_HARD_TIMER_TIM->DIER |= TIM_FLAG_CC3;                      /* 使能CC3中断 */
            break;
        case bsp::detail::HardTimerChannel::k4:
            hard_timer_cb[3] = std::move(call_back);

            BSP_HARD_TIMER_TIM->CCR4 = cnt_tar;                            /* 设置捕获比较计数器CC4 */
            BSP_HARD_TIMER_TIM->SR = static_cast<uint16_t>(~TIM_FLAG_CC4); /* 清除CC4中断标志 */
            BSP_HARD_TIMER_TIM->DIER |= TIM_FLAG_CC4;                      /* 使能CC4中断 */
            break;

        //    case bsp::detail::HardTimerChannel::k5:
        //      hard_timer_cb[4] = std::move(call_back);
        //
        //      BSP_HARD_TIMER_TIM->CCR5 = cnt_tar;                            /* 设置捕获比较计数器CC5 */
        //      BSP_HARD_TIMER_TIM->SR = static_cast<uint16_t>(~TIM_FLAG_CC5); /* 清除CC5中断标志 */
        //      BSP_HARD_TIMER_TIM->DIER |= TIM_FLAG_CC5;                      /* 使能CC5中断 */
        //      break;
        //    case bsp::detail::HardTimerChannel::k6:
        //      hard_timer_cb[5] = std::move(call_back);
        //
        //      BSP_HARD_TIMER_TIM->CCR6 = cnt_tar;                            /* 设置捕获比较计数器CC6 */
        //      BSP_HARD_TIMER_TIM->SR = static_cast<uint16_t>(~TIM_FLAG_CC6); /* 清除CC6中断标志 */
        //      BSP_HARD_TIMER_TIM->DIER |= TIM_FLAG_CC6;                      /* 使能CC6中断 */
        //      break;
        default:
            osError("illegal timer channel");
    }
}

void HardTimer::Delay(HardTimer::Channel channel, units::time::microsecond_t time) {
    Start(channel, time, [&]() { UNUSED() hard_timer_sem[static_cast<uint8_t>(channel) - 1].give(); });
    UNUSED() hard_timer_sem[static_cast<uint8_t>(channel) - 1].take();
}
}  // namespace os

extern "C" void TIM8_CC_IRQHandler(void) {
    uint16_t it_status = 0x0;
    uint16_t it_enable = 0x0;

    it_status = BSP_HARD_TIMER_TIM->SR & TIM_FLAG_CC1;
    it_enable = BSP_HARD_TIMER_TIM->DIER & TIM_FLAG_CC1;

    /* 避免 “千年虫” */
    if (BSP_HARD_TIMER_TIM->SR & TIM_IT_UPDATE) {
        BSP_HARD_TIMER_TIM->SR = (uint16_t)~TIM_IT_UPDATE;
    }

    if (it_status != (uint16_t)RESET) {
        BSP_HARD_TIMER_TIM->SR = (uint16_t)~TIM_FLAG_CC1;
        BSP_HARD_TIMER_TIM->DIER &= (uint16_t)~TIM_FLAG_CC1; /* 禁能CC1中断 */

        /* 先关闭中断，再执行回调函数。因为回调函数可能需要重启定时器 */
        if (it_enable != (uint16_t)RESET) {
            os::hard_timer_cb[0]();
        }
    }

    it_status = BSP_HARD_TIMER_TIM->SR & TIM_FLAG_CC2;
    it_enable = BSP_HARD_TIMER_TIM->DIER & TIM_FLAG_CC2;
    if (it_status != (uint16_t)RESET) {
        BSP_HARD_TIMER_TIM->SR = (uint16_t)~TIM_FLAG_CC2;
        BSP_HARD_TIMER_TIM->DIER &= (uint16_t)~TIM_FLAG_CC2; /* 禁能CC2中断 */

        /* 先关闭中断，再执行回调函数。因为回调函数可能需要重启定时器 */
        if (it_enable != (uint16_t)RESET) {
            os::hard_timer_cb[1]();
        }
    }

    it_status = BSP_HARD_TIMER_TIM->SR & TIM_FLAG_CC3;
    it_enable = BSP_HARD_TIMER_TIM->DIER & TIM_FLAG_CC3;
    if (it_status != (uint16_t)RESET) {
        BSP_HARD_TIMER_TIM->SR = (uint16_t)~TIM_FLAG_CC3;
        BSP_HARD_TIMER_TIM->DIER &= (uint16_t)~TIM_FLAG_CC3; /* 禁能CC2中断 */

        /* 先关闭中断，再执行回调函数。因为回调函数可能需要重启定时器 */
        if (it_enable != (uint16_t)RESET) {
            os::hard_timer_cb[2]();
        }
    }

    it_status = BSP_HARD_TIMER_TIM->SR & TIM_FLAG_CC4;
    it_enable = BSP_HARD_TIMER_TIM->DIER & TIM_FLAG_CC4;
    if (it_status != (uint16_t)RESET) {
        BSP_HARD_TIMER_TIM->SR = (uint16_t)~TIM_FLAG_CC4;
        BSP_HARD_TIMER_TIM->DIER &= (uint16_t)~TIM_FLAG_CC4; /* 禁能CC4中断 */

        /* 先关闭中断，再执行回调函数。因为回调函数可能需要重启定时器 */
        if (it_enable != (uint16_t)RESET) {
            os::hard_timer_cb[3]();
        }
    }

    //  it_status = BSP_HARD_TIMER_TIM->SR & TIM_FLAG_CC5;
    //  it_enable = BSP_HARD_TIMER_TIM->DIER & TIM_FLAG_CC5;
    //  if (it_status != (uint16_t)RESET) {
    //    BSP_HARD_TIMER_TIM->SR = (uint16_t)~TIM_FLAG_CC5;
    //    BSP_HARD_TIMER_TIM->DIER &= (uint16_t)~TIM_FLAG_CC5; /* 禁能CC5中断 */
    //
    //    /* 先关闭中断，再执行回调函数。因为回调函数可能需要重启定时器 */
    //    if (it_enable != (uint16_t)RESET) {
    //      sheriff_os::hard_timer_cb[4]();
    //    }
    //  }
    //
    //  it_status = BSP_HARD_TIMER_TIM->SR & TIM_FLAG_CC6;
    //  it_enable = BSP_HARD_TIMER_TIM->DIER & TIM_FLAG_CC6;
    //  if (it_status != (uint16_t)RESET) {
    //    BSP_HARD_TIMER_TIM->SR = (uint16_t)~TIM_FLAG_CC6;
    //    BSP_HARD_TIMER_TIM->DIER &= (uint16_t)~TIM_FLAG_CC6; /* 禁能CC6中断 */
    //
    //    /* 先关闭中断，再执行回调函数。因为回调函数可能需要重启定时器 */
    //    if (it_enable != (uint16_t)RESET) {
    //      sheriff_os::hard_timer_cb[5]();
    //    }
    //  }
}